#include "ImageProc.h"
#include "mylog.h"
#include <time.h>

extern "C" {
#include "sonix_xu_ctrls.h";
}

#include "digital_data.h"

#define max(a, b)            (((a) > (b)) ? (a) : (b))
#define min(a, b)            (((a) < (b)) ? (a) : (b))

int Dbg_Param = 0x1f;
struct H264Format *gH264fmt = NULL;

METHODDEF(void) my_error_exit(j_common_ptr cinfo) {
    my_error_ptr myerr = (my_error_ptr) cinfo->err;
    (*cinfo->err->output_message)(cinfo);
    longjmp(myerr->setjmp_buffer, 1);
}

GLOBAL(void) jpeg_memory_src(j_decompress_ptr cinfo, void *data, unsigned long len) {
    memory_src_ptr src;

    if (cinfo->src == NULL) {
        cinfo->src = (struct jpeg_source_mgr *) (*cinfo->mem->alloc_small)((j_common_ptr) cinfo,
                                                                           JPOOL_PERMANENT,
                                                                           sizeof(memory_source_mgr));
    }

    src = (memory_src_ptr) cinfo->src;

    src->pub.init_source = memory_init_source;
    src->pub.fill_input_buffer = memory_fill_input_buffer;
    src->pub.skip_input_data = memory_skip_input_data;
    src->pub.resync_to_restart = jpeg_resync_to_restart;
    src->pub.term_source = memory_term_source;
    src->pub.bytes_in_buffer = 0;
    src->pub.next_input_byte = (JOCTET *) data;

    src->skip = 0;
}

METHODDEF(void) memory_init_source(j_decompress_ptr cinfo) {
}

METHODDEF(boolean) memory_fill_input_buffer(j_decompress_ptr cinfo) {
    return FALSE;
}

METHODDEF(void) memory_skip_input_data(j_decompress_ptr cinfo, long num_bytes) {
    memory_src_ptr src = (memory_src_ptr) cinfo->src;

    if (num_bytes > (long) src->pub.bytes_in_buffer) {
        src->skip = (int) (num_bytes - src->pub.bytes_in_buffer);
        src->pub.next_input_byte += src->pub.bytes_in_buffer;
        src->pub.bytes_in_buffer = 0;
    } else {
        src->pub.next_input_byte += (size_t) num_bytes;
        src->pub.bytes_in_buffer -= (size_t) num_bytes;
        src->skip = 0;
    }
}

METHODDEF(void) memory_term_source(j_decompress_ptr cinfo) {
}

static
unsigned char my_jpeg_odml_dht[0x1a4] = {0xff, 0xc4, 0x01, 0xa2,

                                         0x00, 0x00, 0x01, 0x05, 0x01, 0x01, 0x01, 0x01, 0x01, 0x01,
                                         0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x01, 0x02,
                                         0x03, 0x04, 0x05, 0x06, 0x07,
                                         0x08, 0x09, 0x0a, 0x0b,

                                         0x01, 0x00, 0x03, 0x01, 0x01, 0x01, 0x01, 0x01, 0x01, 0x01,
                                         0x01, 0x01, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x01, 0x02,
                                         0x03, 0x04, 0x05,
                                         0x06, 0x07, 0x08, 0x09, 0x0a, 0x0b,

                                         0x10, 0x00, 0x02, 0x01, 0x03, 0x03, 0x02, 0x04, 0x03, 0x05,
                                         0x05, 0x04, 0x04, 0x00, 0x00, 0x01, 0x7d, 0x01, 0x02, 0x03,
                                         0x00, 0x04, 0x11,
                                         0x05, 0x12, 0x21, 0x31, 0x41, 0x06, 0x13, 0x51, 0x61, 0x07,
                                         0x22, 0x71, 0x14, 0x32, 0x81, 0x91, 0xa1, 0x08, 0x23, 0x42,
                                         0xb1, 0xc1, 0x15,
                                         0x52, 0xd1, 0xf0, 0x24, 0x33, 0x62, 0x72, 0x82, 0x09, 0x0a,
                                         0x16, 0x17, 0x18, 0x19, 0x1a, 0x25, 0x26, 0x27, 0x28, 0x29,
                                         0x2a, 0x34, 0x35,
                                         0x36, 0x37, 0x38, 0x39, 0x3a, 0x43, 0x44, 0x45, 0x46, 0x47,
                                         0x48, 0x49, 0x4a, 0x53, 0x54, 0x55, 0x56, 0x57, 0x58, 0x59,
                                         0x5a, 0x63, 0x64,
                                         0x65, 0x66, 0x67, 0x68, 0x69, 0x6a, 0x73, 0x74, 0x75, 0x76,
                                         0x77, 0x78, 0x79, 0x7a, 0x83, 0x84, 0x85, 0x86, 0x87, 0x88,
                                         0x89, 0x8a, 0x92,
                                         0x93, 0x94, 0x95, 0x96, 0x97, 0x98, 0x99, 0x9a, 0xa2, 0xa3,
                                         0xa4, 0xa5, 0xa6, 0xa7, 0xa8, 0xa9, 0xaa, 0xb2, 0xb3, 0xb4,
                                         0xb5, 0xb6, 0xb7,
                                         0xb8, 0xb9, 0xba, 0xc2, 0xc3, 0xc4, 0xc5, 0xc6, 0xc7, 0xc8,
                                         0xc9, 0xca, 0xd2, 0xd3, 0xd4, 0xd5, 0xd6, 0xd7, 0xd8, 0xd9,
                                         0xda, 0xe1, 0xe2,
                                         0xe3, 0xe4, 0xe5, 0xe6, 0xe7, 0xe8, 0xe9, 0xea, 0xf1, 0xf2,
                                         0xf3, 0xf4, 0xf5, 0xf6, 0xf7, 0xf8, 0xf9, 0xfa,

                                         0x11, 0x00, 0x02, 0x01, 0x02, 0x04, 0x04, 0x03, 0x04, 0x07,
                                         0x05, 0x04, 0x04, 0x00, 0x01, 0x02, 0x77, 0x00, 0x01, 0x02,
                                         0x03, 0x11, 0x04,
                                         0x05, 0x21, 0x31, 0x06, 0x12, 0x41, 0x51, 0x07, 0x61, 0x71,
                                         0x13, 0x22, 0x32, 0x81, 0x08, 0x14, 0x42, 0x91, 0xa1, 0xb1,
                                         0xc1, 0x09, 0x23,
                                         0x33, 0x52, 0xf0, 0x15, 0x62, 0x72, 0xd1, 0x0a, 0x16, 0x24,
                                         0x34, 0xe1, 0x25, 0xf1, 0x17, 0x18, 0x19, 0x1a, 0x26, 0x27,
                                         0x28, 0x29, 0x2a,
                                         0x35, 0x36, 0x37, 0x38, 0x39, 0x3a, 0x43, 0x44, 0x45, 0x46,
                                         0x47, 0x48, 0x49, 0x4a, 0x53, 0x54, 0x55, 0x56, 0x57, 0x58,
                                         0x59, 0x5a, 0x63,
                                         0x64, 0x65, 0x66, 0x67, 0x68, 0x69, 0x6a, 0x73, 0x74, 0x75,
                                         0x76, 0x77, 0x78, 0x79, 0x7a, 0x82, 0x83, 0x84, 0x85, 0x86,
                                         0x87, 0x88, 0x89,
                                         0x8a, 0x92, 0x93, 0x94, 0x95, 0x96, 0x97, 0x98, 0x99, 0x9a,
                                         0xa2, 0xa3, 0xa4, 0xa5, 0xa6, 0xa7, 0xa8, 0xa9, 0xaa, 0xb2,
                                         0xb3, 0xb4, 0xb5,
                                         0xb6, 0xb7, 0xb8, 0xb9, 0xba, 0xc2, 0xc3, 0xc4, 0xc5, 0xc6,
                                         0xc7, 0xc8, 0xc9, 0xca, 0xd2, 0xd3, 0xd4, 0xd5, 0xd6, 0xd7,
                                         0xd8, 0xd9, 0xda,
                                         0xe2, 0xe3, 0xe4, 0xe5, 0xe6, 0xe7, 0xe8, 0xe9, 0xea, 0xf2,
                                         0xf3, 0xf4, 0xf5, 0xf6, 0xf7, 0xf8, 0xf9, 0xfa};

static
int
my_jpeg_load_dht(struct jpeg_decompress_struct *info, unsigned char *dht, JHUFF_TBL *ac_tables[],
                 JHUFF_TBL *dc_tables[]) {
    unsigned int length = (dht[2] << 8) + dht[3] - 2;
    unsigned int pos = 4;
    unsigned int count, i;
    int index;

    JHUFF_TBL **hufftbl;
    unsigned char bits[17];
    unsigned char huffval[256];

    while (length > 16) {
        bits[0] = 0;
        index = dht[pos++];
        count = 0;
        for (i = 1; i <= 16; ++i) {
            bits[i] = dht[pos++];
            count += bits[i];
        }
        length -= 17;

        if (count > 256 || count > length)
            return -1;

        for (i = 0; i < count; ++i)
            huffval[i] = dht[pos++];
        length -= count;

        if (index & 0x10) {
            index -= 0x10;
            hufftbl = &ac_tables[index];
        } else
            hufftbl = &dc_tables[index];

        if (index < 0 || index >= NUM_HUFF_TBLS)
            return -1;

        if (*hufftbl == NULL)
            *hufftbl = jpeg_alloc_huff_table((j_common_ptr) info);
        if (*hufftbl == NULL)
            return -1;

        memcpy((*hufftbl)->bits, bits, sizeof(*hufftbl)->bits);
        memcpy((*hufftbl)->huffval, huffval, sizeof(*hufftbl)->huffval);
    }

    if (length != 0)
        return -1;

    return 0;
}

Imageproc::Imageproc(void) {
    fd = -1;
    buffers = NULL;
    n_buffers = 0;
    rgb = NULL;

    readbuffer.start = NULL;
    readbuffer.length = 0;
    readbuffer.buflen = 0;

    img_convert_ctx_MJPEG = NULL;
    out_buffer_MJPEG = NULL;

    if (initffmpeg() == 0)
        ffmpeginit = true;
    else
        ffmpeginit = false;

    substream = false;

    width = 1280;
    height = 720;
}

Imageproc::~Imageproc(void) {
    if (readbuffer.start)
        delete readbuffer.start;

    if (img_convert_ctx_MJPEG)
        sws_freeContext(img_convert_ctx_MJPEG);

    if (out_buffer_MJPEG)
        delete out_buffer_MJPEG;

    av_free_packet(&packet);
    av_frame_free(&pFrame);
    av_frame_free(&pFrameRGB);
    avcodec_free_context(&pCodecCtx_MJPEG);
    av_parser_close(pCodecParserCtx_MJPEG);
}

int Imageproc::errnoexit(const char *s) {
    LOGE("%s error %d, %s", s, errno, strerror(errno));
    return ERROR_LOCAL;
}

int Imageproc::xioctl(int fd, int request, void *arg) {
    int r;

    do
        r = ioctl(fd, request, arg);
    while (-1 == r && EINTR == errno);

    return r;
}

int Imageproc::opendevice(string camname) {
    struct stat st;

    dev_name = camname;
    LOGE("opening %s", dev_name.c_str());

    if (-1 == stat(dev_name.c_str(), &st)) {
        LOGE("Cannot identify '%s': %d, %s", dev_name.c_str(), errno, strerror(errno));
        return ERROR_LOCAL;
    }

    if (!S_ISCHR(st.st_mode)) {
        LOGE("%s is no device", dev_name.c_str());
        return ERROR_LOCAL;
    }

//	fd[camid] = open (dev_name.c_str()[camid], O_RDWR | O_NONBLOCK, 0);
    fd = open(dev_name.c_str(), O_RDWR, 0);

    if (-1 == fd) {
        LOGE("Cannot open '%s': %d, %s", dev_name.c_str(), errno, strerror(errno));
        return ERROR_LOCAL;
    }
    return SUCCESS_LOCAL;
}

int Imageproc::initdevice() {
    unsigned int min;

    int ret;

    CLEAR(fmtdesc);

    v4l2_fmtlist.clear();
    fmtdesc.index = 0;
    fmtdesc.type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
    LOGI("Support format:");
    while (ioctl(fd, VIDIOC_ENUM_FMT, &fmtdesc) != -1) {
        v4l2_fmtlist.push_back(fmtdesc.pixelformat);
        LOGI("%d.%s", fmtdesc.index + 1, fmtdesc.description);
        fmtdesc.index++;
        //if (fmtdesc.pixelformat == V4L2_PIX_FMT_MJPEG) {
        //	return ERROR_LOCAL;
        //}
    }

    CLEAR(cap);

    //获取摄像头参数
    if (-1 == xioctl(fd, VIDIOC_QUERYCAP, &cap)) {
        if (EINVAL == errno) {
            LOGE("%s is no V4L2 device", dev_name.c_str());
            return ERROR_LOCAL;
        } else {
            return errnoexit("VIDIOC_QUERYCAP");
        }
    }

    if (!(cap.capabilities & V4L2_CAP_VIDEO_CAPTURE)) {
        LOGE("%s is no video capture device", dev_name.c_str());
        return ERROR_LOCAL;
    }

    if (!(cap.capabilities & V4L2_CAP_STREAMING)) {
        LOGE("%s does not support streaming i/o", dev_name.c_str());
        return ERROR_LOCAL;
    }

    LOGI("Driver Name:%s	Card Name:%s	Bus info:%s		Driver Version:%u.%u.%u", cap.driver,
         cap.card, cap.bus_info, (cap.version >> 16) & 0xFF, (cap.version >> 8) & 0xFF,
         cap.version & 0xFF);

    LOGI("设置前格式信息:");
    getdevice();

    CLEAR(cropcap);

    cropcap.type = V4L2_BUF_TYPE_VIDEO_CAPTURE;

    if (0 == xioctl(fd, VIDIOC_CROPCAP, &cropcap)) {
        crop.type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
        crop.c = cropcap.defrect;

        if (-1 == xioctl(fd, VIDIOC_S_CROP, &crop)) {
            switch (errno) {
                case EINVAL:
                    break;
                default:
                    break;
            }
        }
    } else {
    }

    CLEAR(fmt);
    //设置帧格式
    fmt.type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
    for (vector<__u32>::iterator it = v4l2_fmtlist.begin(); it != v4l2_fmtlist.end(); it++) {
        if ((*it) == V4L2_PIX_FMT_MJPEG) {//V4L2_PIX_FMT_MJPEG  V4L2_PIX_FMT_YUYV
            fmt.fmt.pix.width = width;
            fmt.fmt.pix.height = height;
            //fmt.fmt.pix.width = 640;
            //fmt.fmt.pix.height = 480;
            fmt.fmt.pix.pixelformat = (*it);
            break;
        }
        if ((*it) == V4L2_PIX_FMT_H264) {
            fmt.fmt.pix.width = width;
            fmt.fmt.pix.height = height;
            //fmt.fmt.pix.width = 640;
            //fmt.fmt.pix.height = 480;
            // 佛山
            fmt.fmt.pix.pixelformat = (*it);
            break;
        }
    }
    fmt.fmt.pix.field = V4L2_FIELD_INTERLACED;

    if (-1 == xioctl(fd, VIDIOC_S_FMT, &fmt))
        return errnoexit("VIDIOC_S_FMT");

    if (fmt.fmt.pix.pixelformat == V4L2_PIX_FMT_H264) {
        //set framerate
        CLEAR(setfps);

        setfps.type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
        setfps.parm.capture.timeperframe.numerator = 1;
        setfps.parm.capture.timeperframe.denominator = 25;
        xioctl(fd, VIDIOC_S_PARM, &setfps);
    } else if (fmt.fmt.pix.pixelformat == V4L2_PIX_FMT_MJPEG) {
        //set framerate
        CLEAR(setfps);

        setfps.type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
        setfps.parm.capture.timeperframe.numerator = 1;
        setfps.parm.capture.timeperframe.denominator = 25;
        xioctl(fd, VIDIOC_S_PARM, &setfps);
    } /*else {
	    LOGI("setfps.parm.capture.timeperframe.denominator = 15;");
		//set framerate
		CLEAR(setfps);

		setfps.type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
		setfps.parm.capture.timeperframe.numerator = 1;
		setfps.parm.capture.timeperframe.denominator = 15;
		xioctl(fd, VIDIOC_S_PARM, &setfps);
	}*/

    min = fmt.fmt.pix.width * 2;
    if (fmt.fmt.pix.bytesperline < min)
        fmt.fmt.pix.bytesperline = min;
    min = fmt.fmt.pix.bytesperline * fmt.fmt.pix.height;
    if (fmt.fmt.pix.sizeimage < min)
        fmt.fmt.pix.sizeimage = min;

    LOGI("设置后格式信息:");
    getdevice();

    time_t t = time(NULL);
    struct tm tmnow;
    char timestr[13];
    if (NULL != localtime_r(&t, &tmnow)) {
        //校准时间
//        XU_OSD_Set_RTC(fd, tmnow.tm_year + 1900, tmnow.tm_mon + 1, tmnow.tm_mday, tmnow.tm_hour,
//                       tmnow.tm_min, tmnow.tm_sec);
    }
    return initmmap();

}

int Imageproc::getdevice() {
    CLEAR(fmt);
    fmt.type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
    ioctl(fd, VIDIOC_G_FMT, &fmt);

    CLEAR(setfps);
    setfps.type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
    xioctl(fd, VIDIOC_G_PARM, &setfps);

    CLEAR(fmtdesc);
    fmtdesc.index = 0;
    fmtdesc.type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
    while (ioctl(fd, VIDIOC_ENUM_FMT, &fmtdesc) != -1) {
        if (fmtdesc.pixelformat == fmt.fmt.pix.pixelformat) {
            LOGI("width:%d	height:%d	format:%s	numerator:%d	denominator:%d",
                 fmt.fmt.pix.width, fmt.fmt.pix.height, fmtdesc.description,
                 setfps.parm.capture.timeperframe.numerator,
                 setfps.parm.capture.timeperframe.denominator);
            break;
        }
        fmtdesc.index++;
    }
    return SUCCESS_LOCAL;
}

int Imageproc::initmmap() {
    struct v4l2_requestbuffers req;

    CLEAR(req);

    req.count = 4;
    req.type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
    req.memory = V4L2_MEMORY_MMAP;

    //分配内存
    if (-1 == xioctl(fd, VIDIOC_REQBUFS, &req)) {
        if (EINVAL == errno) {
            LOGE("%s does not support memory mapping", dev_name.c_str());
            return ERROR_LOCAL;
        } else {
            return errnoexit("VIDIOC_REQBUFS");
        }
    }

    if (req.count < 2) {
        LOGE("Insufficient buffer memory on %s", dev_name.c_str());
        return ERROR_LOCAL;
    }

    //分配缓存
    buffers = (buffer *) calloc(req.count, sizeof(struct buffer));

    if (!buffers) {
        LOGE("Out of memory");
        return ERROR_LOCAL;
    }

    for (n_buffers = 0; n_buffers < req.count; ++n_buffers) {
        struct v4l2_buffer buf;

        CLEAR(buf);

        buf.type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
        buf.memory = V4L2_MEMORY_MMAP;
        buf.index = n_buffers;

        if (-1 == xioctl(fd, VIDIOC_QUERYBUF, &buf))
            return errnoexit("VIDIOC_QUERYBUF");

        //设置映射方式为mmap
        buffers[n_buffers].length = buf.length;
        buffers[n_buffers].start = mmap(NULL, buf.length, PROT_READ | PROT_WRITE, MAP_SHARED, fd,
                                        buf.m.offset);

        if (MAP_FAILED == buffers[n_buffers].start)
            return errnoexit("mmap");
    }

    return SUCCESS_LOCAL;
}

int Imageproc::startcapturing() {
    unsigned int i;
    enum v4l2_buf_type type;

    /*将申请到的帧缓冲全部入队列，以便存放采集到的数据*/
    for (i = 0; i < n_buffers; ++i) {
        struct v4l2_buffer buf;

        CLEAR(buf);

        buf.type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
        buf.memory = V4L2_MEMORY_MMAP;
        buf.index = i;

        //放入缓存
        if (-1 == xioctl(fd, VIDIOC_QBUF, &buf)) {
            LOGE("in startcapturing");
            return errnoexit("VIDIOC_QBUF");
        }
    }

    type = V4L2_BUF_TYPE_VIDEO_CAPTURE;

    //打开视频流
    if (-1 == xioctl(fd, VIDIOC_STREAMON, &type)) {
        return errnoexit("VIDIOC_STREAMON");
    }

    return SUCCESS_LOCAL;
}

int Imageproc::readframeonce() {
    int j;
    for (j = 0; j < 1; j++) {
        fd_set fds;
        struct timeval tv;
        int r;

        FD_ZERO(&fds);
        FD_SET(fd, &fds);

        // Timeout
        tv.tv_sec = 2;
        tv.tv_usec = 0;

        r = select(fd + 1, &fds, NULL, NULL, &tv);

        if (-1 == r) {
            if (EINTR == errno) {
                continue;
            }
            return errnoexit("select");
        }

        if (0 == r) {
            LOGE("select timeout");
            return ERROR_LOCAL;
        }

        if (readframe() == 1) {
            return SUCCESS_LOCAL;
        }
        //sleep(1);
    }

    return ERROR_LOCAL;
}

int Imageproc::setWatermark(int index, string params, int x, int y, int s) {
    if (index >= 0 && index < WATERMARK_SIZE) {
        mWaterMarkS[index].mMarkMtx.write();
        mWaterMarkS[index].Watermark = params;
        mWaterMarkS[index].MarkX = x;
        mWaterMarkS[index].MarkY = y;
        mWaterMarkS[index].MarkSize = s;
        mWaterMarkS[index].mMarkMtx.unlock();
    }
    return SUCCESS_LOCAL;
}

bool Imageproc::addwatermark(AVFrame *pframe) {

    int width = pframe->width;
    int height = pframe->height;
    uint8_t *_ptr = pframe->data[0];

    for (int m = 0; m < WATERMARK_SIZE; m++) {
        mWaterMarkS[m].mMarkMtx.read();
        string mWatermark = mWaterMarkS[m].Watermark;
        int mMarkX = mWaterMarkS[m].MarkX;
        int mMarkY = mWaterMarkS[m].MarkY;
        mWaterMarkS[m].mMarkMtx.unlock();

        vector<const unsigned short *> bitmapDs;
        const char *cmark = mWatermark.c_str();

        if(m == 1){
            bitmapDs.push_back(chinaBityue);
        }

        for (size_t i = 0; i < mWatermark.length(); i++) {
            size_t bmindex = (size_t) cmark[i];

            if (bmindex >= (sizeof(DigitalArray) / sizeof(unsigned short))) {
                bitmapDs.push_back(NULL);
            } else {
                bitmapDs.push_back(DigitalArray[bmindex]);
            }
        }

        if (bitmapDs.size() == 0)
            continue;

        unsigned short rgb16 = 0;

        //绘制图片
        for (int j = 0; j < digital_height; j++) {
            int k = 0;
            for (vector<const unsigned short *>::iterator it = bitmapDs.begin();
                 it != bitmapDs.end(); ++it, k++) {

                const unsigned short *str = (*it);
                if (str != NULL) {
                    //只绘制白色，忽略掉黑色（类似png的通透效果）
                    for (int h = 0; h < digital_width; h++) {
                        rgb16 = *(str + j * digital_width + h);
                        if (rgb16 != 0x00) {
                            *(_ptr + mMarkY * width + mMarkX + j * width +
                              k * (gap_width + digital_width) + h) = rgb16;
                        }
                    }

                }
            }
        }
    }

    return true;
}

bool Imageproc::addwatermark(uint8_t *pframe) {

    int width = fmt.fmt.pix.width;
    int height = fmt.fmt.pix.height;
    uint8_t *_ptr = pframe;

    for (int m = 0; m < WATERMARK_SIZE; m++) {
        mWaterMarkS[m].mMarkMtx.read();
        string mWatermark = mWaterMarkS[m].Watermark;
        int mMarkX = mWaterMarkS[m].MarkX;
        int mMarkY = mWaterMarkS[m].MarkY;
        mWaterMarkS[m].mMarkMtx.unlock();

        vector<const unsigned short *> bitmapDs;
        const char *cmark = mWatermark.c_str();
        for (size_t i = 0; i < mWatermark.length(); i++) {
            size_t bmindex = (size_t) cmark[i];
            if (bmindex >= (sizeof(DigitalArray) / sizeof(unsigned short))) {
                bitmapDs.push_back(NULL);
            } else {
                bitmapDs.push_back(DigitalArray[bmindex]);
            }
        }

        if (bitmapDs.size() == 0)
            continue;

        unsigned short rgb16 = 0;

        //绘制图片
        for (int j = 0; j < digital_height; j++) {
            int k = 0;
            for (vector<const unsigned short *>::iterator it = bitmapDs.begin();
                 it != bitmapDs.end(); ++it, k++) {

                const unsigned short *str = (*it);
                if (str != NULL) {
                    //只绘制白色，忽略掉黑色（类似png的通透效果）
                    for (int h = 0; h < digital_width; h++) {
                        rgb16 = *(str + j * digital_width + h);
                        if (rgb16 != 0x00) {
                            *(_ptr + mMarkY * width + mMarkX + j * width +
                              k * (gap_width + digital_width) + h) = rgb16;
                        }
                    }
                }
            }
        }
    }

    return true;
}

void Imageproc::processimage(const void *p, int l) {

    //struct jpeg_decompress_struct mycinfo;
    //struct my_error_mgr myjerr;
    //JSAMPARRAY jpegbuffer = NULL;
    //int row_stride;

    //mycinfo.err = jpeg_std_error(&myjerr.pub);
    //myjerr.pub.error_exit = my_error_exit;
    //if (setjmp(myjerr.setjmp_buffer)) {
    //	jpeg_destroy_decompress(&mycinfo);
    //	return;/*exit(0);*/
    //}
    //jpeg_create_decompress(&mycinfo);

    //jpeg_memory_src(&mycinfo, (void *) p, l);

    //((memory_source_mgr *) mycinfo.src)->pub.next_input_byte = (JOCTET*) p;
    //((memory_source_mgr *) mycinfo.src)->pub.bytes_in_buffer = l;

    //jpeg_read_header(&mycinfo, TRUE);

    //mycinfo.out_color_space = JCS_RGB;
    //mycinfo.dct_method = JDCT_IFAST;
    //mycinfo.jpeg_color_space = JCS_YCbCr;

    //my_jpeg_load_dht(&mycinfo, my_jpeg_odml_dht, mycinfo.ac_huff_tbl_ptrs, mycinfo.dc_huff_tbl_ptrs);

    //jpeg_start_decompress(&mycinfo);

    //row_stride = mycinfo.image_width * mycinfo.num_components;

    //if (jpegbuffer == NULL) {
    //	jpegbuffer = (*mycinfo.mem->alloc_sarray)((j_common_ptr) &mycinfo, JPOOL_IMAGE, row_stride, 1);
    //}
    //if (jpegbuffer == NULL) {
    //	LOGE("jpegbuffer == NULL");
    //}

    //int y = 0;
    //int *outp = rgb;
    //while (mycinfo.output_scanline < mycinfo.image_height) {

    //	jpeg_read_scanlines(&mycinfo, jpegbuffer, 1);

    //	int xx;
    //	int x3;

    //	for (xx = 0, x3 = 0; xx < fmt.fmt.pix.width && x3 < row_stride; xx++, x3 += 3) {
    //		outp[y + xx] = 0xff000000 | jpegbuffer[0][x3 + 2] << 16 | jpegbuffer[0][x3 + 1] << 8 | jpegbuffer[0][x3 + 0];
    //	}

    //	y += fmt.fmt.pix.width;
    //}

    //jpeg_finish_decompress(&mycinfo);
    //jpeg_destroy_decompress(&mycinfo);

    if (!ffmpeginit)
        return;

    int got;
    packet.data = (uint8_t *) p;
    packet.size = l;
    int ret = avcodec_decode_video2(pCodecCtx_MJPEG, pFrame, &got,
                                    &packet);  //decode to AV_PIX_FMT_YUVJ420P  新摄像头是AV_PIX_FMT_YUVJ422P
    if (ret < 0) {
        LOGE("decodec error");
    }

    addwatermark(pFrame);

    //if (!filterinit) {
    //	if (initfilter(pCodecCtx_MJPEG) < 0)
    //		filterinit = false;
    //	else
    //		filterinit = true;
    //} else {
    //	//added by ws for AVfilter start
    //       pFrame->pts = av_frame_get_best_effort_timestamp(pFrame);

    //       //* push the decoded frame into the filtergraph
    //       if (av_buffersrc_add_frame(buffersrc_ctx, pFrame) < 0) {
    //           LOGE("Could not av_buffersrc_add_frame");
    //       }

    //       ret = av_buffersink_get_frame(buffersink_ctx, pFrame);
    //       if (ret < 0) {
    //           LOGE("Could not av_buffersink_get_frame");
    //       }
    //       //added by ws for AVfilter end
    //}


    if (got && substream) {
        //YUV420转换RGB24
        if (img_convert_ctx_MJPEG == NULL) {
            img_convert_ctx_MJPEG = sws_getContext(pCodecCtx_MJPEG->width, pCodecCtx_MJPEG->height,
                                                   pCodecCtx_MJPEG->pix_fmt, 640, 480,
                                                   AV_PIX_FMT_YUVJ420P, SWS_BICUBIC, NULL, NULL,
                                                   NULL);
            out_buffer_MJPEG = new uint8_t[avpicture_get_size(AV_PIX_FMT_YUVJ420P, 640, 480)];
            avpicture_fill((AVPicture *) pFrameRGB, out_buffer_MJPEG, AV_PIX_FMT_YUVJ420P, 640,
                           480);
        }
        pFrameRGB->width = 640;
        pFrameRGB->height = 480;
        sws_scale(img_convert_ctx_MJPEG, (const uint8_t *const *) pFrame->data, pFrame->linesize, 0,
                  pCodecCtx_MJPEG->height, pFrameRGB->data, pFrameRGB->linesize);

        //int *outp = rgb;
        //int xx;
        //int x3;
        //for (int i = 0; i < pFrameRGB->height; i++){
        //	xx = i * pFrameRGB->width;
        //	x3 = xx * 3;
        //	for (int j = 0; j < pFrameRGB->width; j++, x3 += 3){
        //		outp[j + xx] = 0xff000000 | pFrameRGB->data[0][x3 + 2] << 16 | pFrameRGB->data[0][x3 + 1] << 8 | pFrameRGB->data[0][x3 + 0];
        //	}
        //}
    }

}

unsigned int Imageproc::convert_yuv_to_rgb_pixel(int y, int u, int v) {
    unsigned int pixel32 = 0xff000000;
    unsigned char *pixel = (unsigned char *) &pixel32;
    int r, g, b;
    r = y + (1.370705 * (v - 128));
    g = y - (0.698001 * (v - 128)) - (0.337633 * (u - 128));
    b = y + (1.732446 * (u - 128));
    if (r > 255) r = 255;
    if (g > 255) g = 255;
    if (b > 255) b = 255;
    if (r < 0) r = 0;
    if (g < 0) g = 0;
    if (b < 0) b = 0;
    pixel[0] = r;
    pixel[1] = g;
    pixel[2] = b;
    return pixel32;
}

int Imageproc::convert_yuv_to_rgb_buffer(unsigned char *yuv, int *rgb, unsigned int width,
                                         unsigned int height) {
    unsigned int in, out = 0;
    unsigned int pixel_16;
    int y0, u, y1, v;

    for (in = 0; in < width * height * 2; in += 4) {
        pixel_16 = yuv[in + 3] << 24 | yuv[in + 2] << 16 | yuv[in + 1] << 8 | yuv[in + 0];
        y0 = (pixel_16 & 0x000000ff);
        u = (pixel_16 & 0x0000ff00) >> 8;
        y1 = (pixel_16 & 0x00ff0000) >> 16;
        v = (pixel_16 & 0xff000000) >> 24;
        rgb[out++] = convert_yuv_to_rgb_pixel(y0, u, v);
        rgb[out++] = convert_yuv_to_rgb_pixel(y1, u, v);
    }
    return 0;
}

int Imageproc::readframe() {
    struct v4l2_buffer buf;

    CLEAR(buf);

    buf.type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
    buf.memory = V4L2_MEMORY_MMAP;

    //读取
    if (-1 == xioctl(fd, VIDIOC_DQBUF, &buf)) {
        switch (errno) {
            case EAGAIN:
                return 0;
            case EIO:
            default:
                return errnoexit("VIDIOC_DQBUF");
        }
    }

    assert(buf.index < n_buffers);

    //readbuffer.start = buffers[buf.index].start;
    //readbuffer.length = buf.bytesused;
    if (readbuffer.buflen < buf.bytesused) {
        if (readbuffer.start)
            delete readbuffer.start;

        readbuffer.start = new uint8_t[buf.bytesused];
        readbuffer.buflen = buf.bytesused;
    }
    memcpy(readbuffer.start, buffers[buf.index].start, buf.bytesused);
    readbuffer.length = buf.bytesused;

    if (fmt.fmt.pix.pixelformat == V4L2_PIX_FMT_MJPEG) {
        processimage(readbuffer.start, readbuffer.length);
    } else if (fmt.fmt.pix.pixelformat == V4L2_PIX_FMT_YUYV) {
        convert_yuv_to_rgb_buffer((unsigned char *) readbuffer.start, rgb, fmt.fmt.pix.width,
                                  fmt.fmt.pix.height);
    }

    //放回缓存
    if (-1 == xioctl(fd, VIDIOC_QBUF, &buf)) {
        LOGE("in readframe");
        return errnoexit("VIDIOC_QBUF");
    }

    return 1;
}

int Imageproc::stopcapturing() {
    enum v4l2_buf_type type;

    type = V4L2_BUF_TYPE_VIDEO_CAPTURE;

    //关闭视频流
    if (-1 == xioctl(fd, VIDIOC_STREAMOFF, &type))
        return errnoexit("VIDIOC_STREAMOFF");

    return SUCCESS_LOCAL;

}

int Imageproc::uninitdevice() {
    unsigned int i;

    //释放存储空间
    for (i = 0; i < n_buffers; ++i)
        if (-1 == munmap(buffers[i].start, buffers[i].length))
            return errnoexit("munmap");

    free(buffers);

    return SUCCESS_LOCAL;
}

int Imageproc::closedevice() {
    if (-1 == close(fd)) {
        fd = -1;
        return errnoexit("close");
    }

    fd = -1;
    return SUCCESS_LOCAL;
}

int Imageproc::setWatermark(int group, char *info) {
    //XU_OSD_Set_Multi_Size(fd, 5, 5, 5);
    //XU_OSD_Set_MS_Start_Position(fd, 1, 0, 0);
//    XU_OSD_Set_String(fd, group, info);

    return SUCCESS_LOCAL;
}


int Imageproc::initffmpeg() {
    // 寻找解码器
    av_register_all();

    pCodec_MJPEG = avcodec_find_decoder(AV_CODEC_ID_MJPEG);
    if (pCodec_MJPEG == NULL) {
        LOGE("avcode find decoder failed!");
        return -1;
    }

    pCodecCtx_MJPEG = avcodec_alloc_context3(pCodec_MJPEG);

    //初始化AVCodecParserContext
    pCodecParserCtx_MJPEG = av_parser_init(AV_CODEC_ID_MJPEG);
    if (!pCodecParserCtx_MJPEG) {
        LOGE("AVCodecParseContext error");
        return -1;
    }

    /* we do not send complete frames */
    if (pCodec_MJPEG->capabilities & AV_CODEC_CAP_TRUNCATED)
        pCodecCtx_MJPEG->flags |= AV_CODEC_FLAG_TRUNCATED;

    //打开解码器
    if (avcodec_open2(pCodecCtx_MJPEG, pCodec_MJPEG, NULL) < 0) {
        LOGE("avcode open failed!");
        return -1;
    }

    //为每帧图像分配内存
    pFrame = av_frame_alloc();

    av_init_packet(&packet);
    packet.size = 0;
    packet.data = NULL;

    pFrameRGB = av_frame_alloc();
    return 0;
}

const char *filters_descr = "lutyuv='u=128:v=128'";
//const char *filters_descr = "hflip";
//const char *filters_descr = "hue='h=60:s=-3'";
//const char *filters_descr = "crop=2/3*in_w:2/3*in_h";
//const char *filters_descr = "drawbox=x=200:y=200:w=300:h=300:color=pink@0.5";
//const char *filters_descr = "movie=/storage/emulated/0/ws.jpg[wm];[in][wm]overlay=5:5[out]";
//const char *filters_descr="drawgrid=width=100:height=100:thickness=4:color=pink@0.9";

int Imageproc::initfilter(AVCodecContext *pCodecCtx) {
    //added by ws for AVfilter start----------init AVfilter--------------------------ws
    av_register_all();
    avfilter_register_all();

    char args[512];
    int ret;
    const AVFilter *buffersrc = avfilter_get_by_name("buffer");
    const AVFilter *buffersink = avfilter_get_by_name("buffersink");//新版的ffmpeg库必须为buffersink
    AVFilterInOut *outputs = avfilter_inout_alloc();
    AVFilterInOut *inputs = avfilter_inout_alloc();
    enum AVPixelFormat pix_fmts[] = {AV_PIX_FMT_YUV420P, AV_PIX_FMT_NONE};
    AVBufferSinkParams *buffersink_params;

    filter_graph = avfilter_graph_alloc();

    /* buffer video source: the decoded frames from the decoder will be inserted here. */
    snprintf(args, sizeof(args),
             "video_size=%dx%d:pix_fmt=%d:time_base=%d/%d:pixel_aspect=%d/%d",
             pCodecCtx->width, pCodecCtx->height, pCodecCtx->pix_fmt,
            /*pCodecCtx->time_base.num*/1, /*pCodecCtx->time_base.den*/15,
            /*pCodecCtx->sample_aspect_ratio.num*/1, /*pCodecCtx->sample_aspect_ratio.den*/15);

    ret = avfilter_graph_create_filter(&buffersrc_ctx, buffersrc, "in",
                                       args, NULL, filter_graph);
    if (ret < 0) {
        LOGE("Cannot create buffer source  %d\n", ret);
        return ret;
    }

    /* buffer video sink: to terminate the filter chain. */
    buffersink_params = av_buffersink_params_alloc();
    buffersink_params->pixel_fmts = pix_fmts;
    ret = avfilter_graph_create_filter(&buffersink_ctx, buffersink, "out",
                                       NULL, buffersink_params, filter_graph);
    av_free(buffersink_params);
    if (ret < 0) {
        LOGE("Cannot create buffer sink\n");
        return ret;
    }

    /* Endpoints for the filter graph. */
    outputs->name = av_strdup("in");
    outputs->filter_ctx = buffersrc_ctx;
    outputs->pad_idx = 0;
    outputs->next = NULL;

    inputs->name = av_strdup("out");
    inputs->filter_ctx = buffersink_ctx;
    inputs->pad_idx = 0;
    inputs->next = NULL;

    // avfilter_link(buffersrc_ctx, 0, buffersink_ctx, 0);

    if ((ret = avfilter_graph_parse_ptr(filter_graph, filters_descr,
                                        &inputs, &outputs, NULL)) < 0) {
        LOGE("Cannot avfilter_graph_parse_ptr\n");
        return ret;
    }

    if ((ret = avfilter_graph_config(filter_graph, NULL)) < 0) {
        LOGE("Cannot avfilter_graph_config\n");
        return ret;
    }

    return 0;
    //added by ws for AVfilter end------------init AVfilter------------------------------ws
}